%% 递推最小二乘估计的融合
clear all;
clc;

%% 参数初始化
k = 100;
realLocation = [100; 200; 300];
tmpHk = [eye(3); eye(3)];
mu = realLocation;
sigma1 = diag([4,4,16]);
sigma2 = eye(3);
sigma = diag([4,4,16,1,1,1]);

%% 融合
tic
Hi = tmpHk;
HiT = tmpHk';
invHTH = inv(tmpHk' * tmpHk);
Ri = sigma;
estimateXpre = realLocation;
%meanSquareErrorPre = invHTH *HiT *Ri *Hi *invHTH;
meanSquareErrorPre = inv(HiT * Ri * Hi);
rng(1);

for i = 1:1:100
    tmp = Hi * meanSquareErrorPre * HiT + Ri;
    gainK = meanSquareErrorPre * HiT / tmp;
    
    Zi = [mvnrnd(mu, sigma1, 1), mvnrnd(mu, sigma2, 1)]';
    
    estimateXk = estimateXpre + gainK*(Zi - Hi*estimateXpre);
    meanSquareErrorK = meanSquareErrorPre - gainK*Hi*meanSquareErrorPre;
    estimateXpre = estimateXk;
    meanSquareErrorPre = meanSquareErrorK;
end
estimateX3 = estimateXk;
errorX3 = estimateX3 - realLocation
meanSquareErrorX3 = meanSquareErrorK
toc